#include "firfilter.h"
#include <cmath>
#include <cstring>
#include <cstdio>


/**
 * Init the filter operation
 */
firfilter::firfilter()
{

}

/**
 * Destructor
 */
firfilter::~firfilter()
{

}

/*
 * Init the filter operation
 */
void firfilter::init(tFirMainStructure* tFirMainStructure_)
{
    for (int i=0; i < MAX_FIR_ORDER; i++)
    {
        tFirMainStructure_->history[i] = 0;
    }

    /* filling structure with initial values */
    tFirMainStructure_->fir_order =
            MAX_FIR_ORDER;
    tFirMainStructure_->firType =
            FIR_COEFF_LOW;
    tFirMainStructure_->firWinType =
            FIR_COEFF_WIND_RECT;
    tFirMainStructure_->fir_fc[0] =
            static_cast<float>(INIT_FREC_C0/SAMPLING_FREQ)*2*M_PI;
    tFirMainStructure_->fir_fc[1] =
            static_cast<float>(INIT_FREC_C1/SAMPLING_FREQ)*2*M_PI;
    tFirMainStructure_->fir_atenuation = 0;

    /* Initialization of Window */
    GetWindow(
                tFirMainStructure_->window,
                tFirMainStructure_->firWinType,
                tFirMainStructure_->fir_order);

    /* After initializing the window the coef are able to be calculated */
    FirCoeff (
                tFirMainStructure_->coefs,
                tFirMainStructure_->fir_order,
                tFirMainStructure_->fir_fc,
                tFirMainStructure_->firType,
                tFirMainStructure_->window);

    tFirMainStructure_->hist_pos = 0;
}

/**
 * IIRFILTER
 * 
 *  y(n) = b_0*x(n) + b_1*x(n-1) + ... + b_29*x(n-29)
 */   
void firfilter::filter(
    int blockSize,                              /* Number of output samples */
    float* in,                                  /* Pointer to input array (n) */
    float* out,                                 /* Pointer to output data (y) */
    tFirMainStructure* tFirMainStructure_)	/* Pointer to Fir structure */
{
    int i;
    int j;
    int hist_pos;
    int fir_order;
    float sum;
    float *pHistory;
    float *pCoefs;

    pHistory = tFirMainStructure_->history;
    pCoefs = tFirMainStructure_->coefs;
    fir_order = tFirMainStructure_->fir_order;

    for (i = 0; i < blockSize; i++)
    {
        hist_pos = tFirMainStructure_->hist_pos;

        pHistory[hist_pos] = in[i];
        tFirMainStructure_->hist_pos++;

        if(tFirMainStructure_->hist_pos == fir_order)
        {
            tFirMainStructure_->hist_pos = 0;
        }

        sum = 0.00;
        for(j=0; j<fir_order; j++)
        {
            sum += (pCoefs[j]) * (pHistory[hist_pos]);
            hist_pos--;

            if(hist_pos < 0)
            {
                hist_pos = fir_order-1;
            }
        }
        out[i] = sum;
    }
}

/*
 * Reset
 */
void firfilter::reset(tFirMainStructure* tFirMainStructure_)
{
    for (int i=0; i < MAX_FIR_ORDER; i++)
    {
        tFirMainStructure_->history[i] = 0;
    }

    tFirMainStructure_->fir_order = MAX_FIR_ORDER;
    tFirMainStructure_->firType = FIR_COEFF_LOW;
    tFirMainStructure_->firWinType = FIR_COEFF_WIND_RECT;
    tFirMainStructure_->fir_fc[0] = static_cast<float>(1000.0/SAMPLING_FREQ)*2*M_PI;
    tFirMainStructure_->fir_fc[1] = static_cast<float>(3000.0/SAMPLING_FREQ)*2*M_PI;
    tFirMainStructure_->fir_atenuation = 0;

    GetWindow(tFirMainStructure_->window,tFirMainStructure_->firWinType,tFirMainStructure_->fir_order);
    FirCoeff (
                tFirMainStructure_->coefs,
                tFirMainStructure_->fir_order,
                tFirMainStructure_->fir_fc,
                tFirMainStructure_->firType,
                tFirMainStructure_->window);

    tFirMainStructure_->hist_pos = 0;
}

void firfilter::GetWindow(
    float *pWindow,
    eFirWindType type,
    int length)
{
    int n;
    float k;

    k=(float)(2*M_PI/(length-1));
    switch (type)
    {
        case FIR_COEFF_WIND_HAMM:
            /* Hamming window */
            for(n = 0; n < length; n++)
            {
                pWindow[n] = (float)(0.54-0.46*cos(k*n));
            }
            break;
        case FIR_COEFF_WIND_BLACK:
            /* Blackman window */
            for(n = 0; n < length; n++)
            {
                pWindow[n] = (float)(0.42-0.5*cos(k*n)+0.08*cos(2*k*n));
            }
            break;
        case FIR_COEFF_WIND_HANN:
            /* Hanning window */
            for(n = 0; n < length; n++)
            {
                pWindow[n] = (float)(0.5)*(1-cos(k*n));
            }
            break;
        case FIR_COEFF_WIND_RECT:
            for(n = 0; n < length; n++)
            {
                pWindow[n] = (float)1;
            }
    }
}

void firfilter::FirCoeff(
    float *pBetaCoeff,
    int order,
    float *pBandEdge,
    eFirType type,
    float *pWind)
{
    int delay;
    int n;
    float omega_cut0;
    float omega_cut1;

    delay = (order - 1) / 2;
    omega_cut0 = pBandEdge[0];
    switch (type)
    {
        case (FIR_COEFF_LOW):

            for (n = 0; n < delay; n++)
            {
                pBetaCoeff[order - n - 1]  = pBetaCoeff[n] = (float)
                    (sin((n - delay) * omega_cut0) /
                    (M_PI * (n - delay)));
            }

            pBetaCoeff[n] = (float)(omega_cut0/M_PI);
            break;

        case (FIR_COEFF_HIGH):
            omega_cut1 = pBandEdge[1];
            for (n = 0; n < delay; n++)
            {
                pBetaCoeff[order - n - 1]  = pBetaCoeff[n] = (float)
                    ((sin((n - delay) * M_PI) -
                      sin((n - delay) * omega_cut1)) /
                    (M_PI * (n - delay)));
            }

            pBetaCoeff[n] = (float)((M_PI - omega_cut1) / M_PI);
            break;
        /* Bandpass filter specification */
        case FIR_COEFF_BAND:
            omega_cut1 = pBandEdge[1];
            for (n = 0; n < delay; n++)
            {
                pBetaCoeff[order - n - 1]  = pBetaCoeff[n] = (float)
                    ((sin((n - delay) * omega_cut1) -
                      sin((n - delay) * omega_cut0)) /
                    (M_PI * (n - delay)));
            }

            pBetaCoeff[n] = (float)((omega_cut1 - omega_cut0) / M_PI);
            break;

        case (FIR_COEFF_STOP):

            omega_cut1 = pBandEdge[1];
            for (n = 0; n < delay; n++)
            {
                pBetaCoeff[order - n - 1]  = pBetaCoeff[n] = (float)
                    ((sin((n - delay) * M_PI) -
                      sin((n - delay) * omega_cut1) +
                      sin((n - delay) * omega_cut0)) /
                    (M_PI * (n - delay)));
            }
            pBetaCoeff[n] = (float)
                ((M_PI - omega_cut1 + omega_cut0) /
                M_PI);
            break;
    }

    /* Verify if window is avalaible */
    if(pWind == NULL)
    {
        return;
    }
    else
    {
        /* Apply window */
        for (n = 0; n < order; n++)
        {
            pBetaCoeff[n] *= pWind[n];
        }
    }
}
